/*
同步写例子在STS3215中测试通过，舵机出厂速度单位为0.0146rpm，舵机机运行速度V=3400
如果使用的出厂速度单位是0.732rpm，则速度改为V=68，延时公式T=[(P1-P0)/(50*V)]*1000+[(50*V)/(A*100)]*1000
注意：那个厂家的调试板子TX和RX并不是芯片上面的，而是他想让我接到单片机上面的，所以标的是反的，把我坑死了
*/
#ifdef U8X8_HAVE_HW_SPI
#include <SPI.h>
#endif
#ifdef U8X8_HAVE_HW_I2C
#include <Wire.h>
#endif
#include <SCServo.h>
#include <FastLED.h>
#include "ESP32Tone.h"
#define BUZZER_PIN 4
#define WS2812_1_DATA_PIN 5
#define NUM_LEDS 1
#define LED_TYPE WS2812 // LED灯带型号
#define COLOR_ORDER GRB // RGB灯珠中红色、绿色、蓝色LED的排列顺序

SMS_STS st;
byte ID[2];
s16 Position[2];
u16 Speed[2];
byte ACC[2];
//蜂鸣器
int pu[5] = {1000, 1000, 1500, 2000, 2500};
int delay_ms[5] = {100, 100, 300, 300, 300};
int notone_delay_ms[] = {100, 300, 100, 100, 100};
//WS2812
uint8_t max_bright = 255; // LED亮度控制变量，可使用数值为 0 ～ 255， 数值越大则光带亮度越高
CRGB leds_1[NUM_LEDS];	  // 建立灯带leds

// pin:蜂鸣器引脚
void BUZZER_beep(int pin)
{
	for (int i = 2; i < 5; i++)
	{
		tone(pin, pu[i] - 1);
		delay(delay_ms[i]);
		noTone(pin);
		delay(notone_delay_ms[i]);
	}
	digitalWrite(pin, HIGH);
	delay(delay_ms[0]);
	digitalWrite(pin, LOW);
	delay(notone_delay_ms[0]);
	digitalWrite(pin, HIGH);
	delay(delay_ms[1]);
	digitalWrite(pin, LOW);
	delay(notone_delay_ms[1]);
}
uint8_t startIndex = 0;
//色板函数
void palette_color()
{
	startIndex++;
	// lED光带对象 灯珠数量 起始颜色序号 灯珠间色差 色板名称 亮度 颜色过渡效果(NOBLEND)
	fill_palette(leds_1, NUM_LEDS, startIndex, 8, LavaColors_p, 255, LINEARBLEND);
	FastLED.show();
}
uint8_t the_color = 0;
uint8_t change_step = 3;

//彩虹色渐变流动
void rain_bow_fluid()
{
  fill_rainbow(leds_1, NUM_LEDS, the_color, change_step);
  FastLED.show();
  the_color++;  // 为什么这个一直加不会超过有效的最大数值，因为这个变量的类
                // 型是uint8_t，最大只会保存到255，所以不会超过255的
  // change_step+=0.1;  // 这一句没啥用
}
void task_LED(void *pvParam) // 宣告任务LED闪烁
{
	/* 使用ledc通道1，设置频率为1kHz，分辨率为10位 */
	int freq = 1000;														  // 频率
	int channel = 0;														  // 通道
	int resolution = 8;														  // 分辨率
	ledcSetup(channel, freq, resolution);									  // 设置通道
	ledcAttachPin(LED_BUILTIN, channel);									  // 将通道与对应的引脚连接
	LEDS.addLeds<LED_TYPE, WS2812_1_DATA_PIN, COLOR_ORDER>(leds_1, NUM_LEDS); // 初始化灯带
	FastLED.setBrightness(max_bright);										  // 设置灯带亮度
	while (1)
	{
		rain_bow_fluid();
		// 逐渐变亮
		for (int dutyCycle = 0; dutyCycle <= 255; dutyCycle = dutyCycle + 5)
		{
			ledcWrite(channel, dutyCycle); // 输出PWM
			vTaskDelay(pdMS_TO_TICKS(20));
		}
		// 逐渐变暗
		for (int dutyCycle = 255; dutyCycle >= 0; dutyCycle = dutyCycle - 5)
		{
			ledcWrite(channel, dutyCycle); // 输出PWM
			vTaskDelay(pdMS_TO_TICKS(20));
		}
	}
}

void setup()
{
	xTaskCreate(task_LED, "task_LED", 1000, NULL, 1, NULL);
	Serial.begin(115200);
	Serial2.begin(1000000);
	st.pSerial = &Serial2;
	delay(1000);
	ID[0] = 1;
	ID[1] = 2;
	Speed[0] = 3400;
	Speed[1] = 3400;
	ACC[0] = 50;
	ACC[1] = 50;
	pinMode(BUZZER_PIN, OUTPUT);
	BUZZER_beep(BUZZER_PIN);
}

void loop()
{
	Serial.println("OK!");
	Position[0] = 4095;
	Position[1] = 4095;
	st.SyncWritePosEx(ID, 2, Position, Speed, ACC); //舵机(ID1/ID2)以最高速度V=3400步/秒，加速度A=50(50*100步/秒^2)，运行至P1=4095位置
	delay(1884);									//((P1-P0)/V)*1000+(V/(A*100))*1000
	Serial.println(Position[1]);
	Position[0] = 0;
	Position[1] = 0;
	st.SyncWritePosEx(ID, 2, Position, Speed, ACC); //舵机(ID1/ID2)以最高速度V=3400步/秒，加速度A=50(50*100步/秒^2)，运行至P0=0位置
	delay(1884);									//((P1-P0)/V)*1000+(V/(A*100))*1000
}
